/*
 * MPU_6050.cpp
 *
 *      Author: Christian
 *      Letztes Update: 03.11.13 Christian | Code Aufräumen
 *
 */

#include "MPU_6050.h"

	int16_t xA_off;
	int16_t yA_off;
	int16_t zA_off;

	int16_t xG_off;
	int16_t yG_off;
	int16_t zG_off;

static int16_t mpu_offset( uint8_t reg ) {
	int16_t tmp  = 0;
	int32_t tmp2 = 0;
	for ( uint8_t i = 0; i < 200; i++ ) {

		mpu_read_value( reg , &tmp);
		tmp2 = tmp2 + tmp;
		_delay_us( 150 );
	}
	return ( tmp2 / 200 );
}

void mpu_init( void ) {

	twi_init();

// Sample Rate Divider einstellen
	mpu_write_register( SMPLRT_DIV, 0x00 );

// EXT-SYNC aus Low-pass-Filter auf Stufe 3
	mpu_write_register( CONFIG, 0x02 );

// Gyro-Config
	mpu_write_register( GYRO_CONFIG, 0x00 ); // Self-Test aus, +-250�/s Range

//Accel-Config
	mpu_write_register( ACCEL_CONFIG, 0x01 ); // Self-Test aus, +-4g Range

// SLEEP Modus ausschalten

	mpu_write_register( PWR_MGMT_1, 0x00 );

// warten bis gyro eingeschwungen
	_delay_ms( 40 );

// MOT_THR und FIFO_EN bleiben 0x00
// INT_PIN_CFG und INT_ENABLE bleiben 0x00

// CLock source auf Gyro X setzen
	mpu_write_register( PWR_MGMT_1, 0x01 );


	xA_off = mpu_offset( ACCEL_XOUT_H );
	yA_off = mpu_offset( ACCEL_YOUT_H );
	zA_off = mpu_offset( ACCEL_ZOUT_H );

	xG_off = mpu_offset( GYRO_XOUT_H );
	yG_off = mpu_offset( GYRO_YOUT_H );
	zG_off = mpu_offset( GYRO_ZOUT_H );


	mpu_ready();
}


uint8_t mpu_self_test( void ) {

/*	uint16_t temp = 0;
	uint16_t str[3] = { 0, 0, 0 };
	uint16_t ft[3] = { 0, 0, 0 };
	uint8_t xyz_test[3] = { 0, 0, 0};

	// Accelerometer:


	// Gyro:

		// STR Bestimmen

		for ( uint8_t i = 0; i < 3; i++)
		{
			mpu_read_value( GYRO_XOUT_H + ( i * 0x02 ) , &temp );
			str[i] = temp;
		}

		mpu_write_register( GYRO_CONFIG, GYRO_SELF_TEST_CONFIG );

		for ( uint8_t i = 0; i < 3; i++)
		{
			mpu_read_value( GYRO_XOUT_H + ( i * 0x02 ) , &temp );
			str[i] = temp - str[i];
		}

		//xyz_test f�r gyro bestimmen
		for ( uint8_t i = 0; i < 3; i++)
		{
			mpu_read_register( SELF_TEST_X + ( i * 0x01 ), xyz_test + i);
			xyz_test[i] &= 0x1F;
		}

		//FT berechnen


*/
	return 1;
}


uint8_t mpu_write_register( uint8_t reg, uint8_t wert ) {

	if( twi_start( MPU_6050_SLAVE_ADDRESS_W ) ) {
		if ( twi_send_data( reg ) ) {
			if ( twi_send_data( wert ) ) {
				twi_stop();
				return 1;
			}
		}
	}
	i2c_error();
	return 0;
}

uint8_t mpu_read_register( uint8_t reg, uint8_t *data ) {

	if ( twi_start( MPU_6050_SLAVE_ADDRESS_W ) ) {
		if ( twi_send_data( reg ) ) {
			if ( twi_repstart( MPU_6050_SLAVE_ADDRESS_R ) ) {
				*data = twi_recieve_data_nack();
				twi_stop();
				return 1;
			}
		}
	}
	i2c_error();
	return 0;
}

uint8_t mpu_read_value( uint8_t reg_h, int16_t *data ) {

	int16_t* temp = 0;
	if ( twi_start( MPU_6050_SLAVE_ADDRESS_W ) ) {
		if ( twi_send_data( reg_h ) ) {
			if ( twi_repstart( MPU_6050_SLAVE_ADDRESS_R ) )	{
				*data = twi_recieve_data_ack();
				*data = *data << 8;
				*temp = twi_recieve_data_nack();
				*data |= *temp;
				twi_stop();
				return 1;
			}
		}
	}
	i2c_error();
	return 0;
}


